#include "project_config.h"

#if (DRIVER_PSU_QSPI == 1)
#include "qelib.h"
#include "xil_cache.h"


/**
 * GQSPI_CFG (QSPI) Register
 */
#define GQSPI_CR_MODE_EN_DMA        QE_BIT(31) 
#define GQSPI_CR_MODE_EN_MASK       QE_GENMASK(31,30)
#define GQSPI_CR_START_MODE_MASK    QE_BIT(29)
#define GQSPI_CR_START_MODE_MANU    QE_BIT(29)
#define GQSPI_CR_START_GENFIFO      QE_BIT(28)
#define GQSPI_CR_ENDIAN             QE_BIT(26)
#define GQSPI_CR_EN_POOL_TO         QE_BIT(20)
#define GQSPI_CR_WP_HOLD            QE_BIT(19)
#define GQSPI_CR_BAUD_DIV_MASK      QE_GENMASK(5,3)
#define GQSPI_CR_BAUD_RATE_DIV      QE_BIT(3)
#define GQSPI_CR_PRESC_MAX          7U
#define GQSPI_CR_BAUD_DIV_SHIFT     3
#define GQSPI_CR_CLK_PHA            QE_BIT(2)
#define GQSPI_CR_CLK_POL            QE_BIT(1)

/**
 * ISR (QSPI) Register 
 */
#define GQSPI_ISR_RX_EMPTY          QE_BIT(11)
#define GQSPI_ISR_GFIFO_FULL        QE_BIT(10)
#define GQSPI_ISR_GFIFO_NOT_FULL    QE_BIT(9)
#define GQSPI_ISR_TX_EMPTY          QE_BIT(8)
#define GQSPI_ISR_GFIFO_EMPTY       QE_BIT(7)
#define GQSPI_ISR_RX_FULL           QE_BIT(5)
#define GQSPI_ISR_RX_NOT_EMPTY      QE_BIT(4)
#define GQSPI_ISR_TX_FULL           QE_BIT(3)
#define GQSPI_ISR_TX_NOT_FULL       QE_BIT(2)
#define GQSPI_ISR_POOL_TIME_EXPIRE  QE_BIT(1)
#define GQSPI_ISR_WTC_MASK          0x00000002U
#define GQSPI_ISR_ALL_MASK          0x00000FBEU

/**
 * GQSPI_En (QSPI) Register
 */
#define GQSPI_EN_ENABLE             QE_BIT(0)

/**
 * GQSPI_IDR (QSPI) Register 
 */
#define GQSPI_IDR_ALL_MASK          0x00000FBEU

/**
 * QSPI_LPBK_DLY_ADJ (QSPI) Register
 */
#define GQSPI_LPBK_DLY_ADJ_LPBK     QE_BIT(5)

/**
 * GQSPI_TX_THRESH (QSPI) Register
 */
#define GQSPI_TX_FIFO_THR_RST_VAL   0x01U

/**
 * GQSPI_RX_THRESH (QSPI) Register
 */
#define GQSPI_RX_FIFO_THR_RST_VAL   0x01U

/**
 * GQSPI_GEN_FIFO (QSPI) Register
 */
#define GQSPI_GENFIFO_POLL          QE_BIT(19)
#define GQSPI_GENFIFO_STRIPE        QE_BIT(18)
#define GQSPI_GENFIFO_RX            QE_BIT(17)
#define GQSPI_GENFIFO_TX            QE_BIT(16)
#define GQSPI_GENFIFO_BUS_UPPER     QE_BIT(15)
#define GQSPI_GENFIFO_BUS_LOWER     QE_BIT(14)
#define GQSPI_GENFIFO_BUS_BOTH      QE_GENMASK(15,14)
#define GQSPI_GENFIFO_BUS_MASK      QE_GENMASK(15,14)
#define GQSPI_GENFIFO_CS_UPPER      QE_BIT(13)
#define GQSPI_GENFIFO_CS_LOWER      QE_BIT(12)
#define GQSPI_GENFIFO_MODE_MASK     QE_GENMASK(11,10)
#define GQSPI_GENFIFO_MODE_QSPI     QE_GENMASK(11,10)
#define GQSPI_GENFIFO_MODE_DSPI     QE_BIT(11)
#define GQSPI_GENFIFO_MODE_SPI      QE_BIT(10)
#define GQSPI_GENFIFO_EXP           QE_BIT(9)
#define GQSPI_GENFIFO_EXP_START	    0x100U
#define GQSPI_GENFIFO_XFER          QE_BIT(8)
#define GQSPI_GENFIFO_IMM_DATA_MASK QE_GENMASK(7,0)
#define GQSPI_GENFIFO_CS_SETUP      0x00000005U
#define GQSPI_GENFIFO_CS_HOLD       0x00000004U  

/**
 * GQSPI_SEL (QSPI) Register 
 */
#define GQSPI_SEL_GQSPI             QE_BIT(0)

/**
 * GQSPI_FIFO_CTRL (QSPI) Register
 */
#define GQSPI_FIFO_CTRL_RST_RX      QE_BIT(2)
#define GQSPI_FIFO_CTRL_RST_TX      QE_BIT(1)
#define GQSPI_FIFO_CTRL_RST_GEN     QE_BIT(0)

/**
 * GQSPI_GF_THRESH (QSPI) Register
 */
#define GQSPI_GEN_FIFO_THR_RST_VAL  0x10U

/**
 * QSPIDMA_DST_CTRL (QSPI) Register
 * 
 */
#define GQSPI_DMA_DST_CTRL_RST_VAL  0x403FFA00U

/**
 * QSPIDMA_DST_STS (QSPI) Register
 */
#define GQSPI_DMA_STS_DONE      QE_BIT(1)
#define GQSPI_DMA_STS_WTC       0x0000E000U

/**
 * QSPIDMA_DST_I_EN (QSPI) Register
 */
#define GQSPI_DMA_IER_ALL_MASK      QE_GENMASK(7,1)



#define GQSPI_DMA_IDR_ALL_MASK      0x000000FEU
#define GQSPI_DMA_DST_ADDR_MASK     0xFFFFFFFCU
#define GQSPI_DMA_DST_ADDR_MSB_MASK 0x00000FFFU
#define GQSPI_FIFO_DEPTH        (64)

#define GQSPI_DFLT_BAUD_RATE_VAL	2
#define GQSPI_MAX_BAUD_RATE_VAL		7

#define ZYNQ_QSPI_BASE              0xFF0F0000
#define GQSPI_REG_OFFSET		    0x100
#define GQSPI_DMA_REG_OFFSET		0x800

#define GQSPI_READMODE_DMA          0
#define GQSPI_READMODE_IO           1
#define GQSPI_CLK_PRESCALE_2		0x00U

#define GQSPI_CLK_PRESCALE_4		0x01U
#define GQSPI_CLK_PRESCALE_8		0x02U
#define GQSPI_CLK_PRESCALE_16		0x03U
#define GQSPI_CLK_PRESCALE_32		0x04U
#define GQSPI_CLK_PRESCALE_64		0x05U
#define GQSPI_CLK_PRESCALE_128	    0x06U
#define GQSPI_CLK_PRESCALE_256	    0x07U

#define GQSPI_DMA_ALIGN             0x4
#define GQSPI_DMA_BYTES_MAX         0x10000000U

#define QSPI_LOGDOMAIN          "zynq-qspi"
#define qspi_debug(...)  	    qelog_debug(QSPI_LOGDOMAIN, __VA_ARGS__)
#define qspi_info(...)   	    qelog_info(QSPI_LOGDOMAIN, __VA_ARGS__)
#define qspi_warning(...)       qelog_warning(QSPI_LOGDOMAIN, __VA_ARGS__)
#define qspi_error(...)  	    qelog_error(QSPI_LOGDOMAIN, __VA_ARGS__)




typedef struct {
    volatile qe_u32 cr;              /* 0x00 Configuration register */
    volatile qe_u32 isr;             /* 0x04 Interrupt Status */
    volatile qe_u32 ier;             /* 0x08 Interrupt Enable */
    volatile qe_u32 idr;             /* 0x0C Interrupt Disable */
    volatile qe_u32 imr;             /* 0x10 Interrupt Un-Mask (enabled) */
    volatile qe_u32 enr;             /* 0x14 LQSPI Enable */
    volatile qe_u32 dr;              /* 0x18 Timing Control Delay */
    volatile qe_u32 txd0r;           /* 0x1C Transmit Data, 4 Bytes */
    volatile qe_u32 drxr;            /* 0x20 Receive Data in RX FIFO */
    volatile qe_u32 sicr;            /* 0x24 Slave Idle Count */
    volatile qe_u32 txftr;           /* 0x28 TX FIFO Threshold */
    volatile qe_u32 rxftr;           /* 0x2C RX FIFO Threshold */
    volatile qe_u32 gpior;           /* 0x30 Write Protection Output */
    volatile qe_u32 reserved0;       /* 0x34 */
    volatile qe_u32 lpbkdly;         /* 0x38 Loopback Master Clock Delay Adjustment */
    volatile qe_u32 reserved1;       /* 0x3C */
    volatile qe_u32 genfifo;         /* 0x40 GQSPI Generic FIFO Configuration */
    volatile qe_u32 gqspisel;        /* 0x44 GQSPI Select */
    volatile qe_u32 reserved2;       /* 0x48 */
    volatile qe_u32 gqfifoctrl;      /* 0x4C GQSPI FIFO Control */
    volatile qe_u32 gfthr;           /* 0x50 GQSPI Generic FIFO Threshold Level */
    volatile qe_u32 gqpollcfg;       /* 0x54 GQSPI Poll Configuration Register */
    volatile qe_u32 gqpollto;        /* 0x58 GQSPI Poll Time out */
    volatile qe_u32 gqxfersts;       /* 0x5C */
    volatile qe_u32 reserved3[38];	/* 0x60 */
    volatile qe_u32 gqspidlyadj;	    /* 0xF8 */
} psu_qspi_regs;

typedef struct {
    volatile qe_u32 dst;             /* 0x00 DMA destination memory address */
    volatile qe_u32 size;            /* 0x04 DMA transfer payload */
    volatile qe_u32 sts;             /* 0x08 General DST DMA Status */
    volatile qe_u32 ctrl;            /* 0x0C General DST DMA Control */
    volatile qe_u32 reserved0;       /* 0x10 */
    volatile qe_u32 isr;             /* 0x14 DST DMA Interrupt Status */
    volatile qe_u32 ier;             /* 0x18 DST DMA Interrupt Enable */
    volatile qe_u32 idr;             /* 0x1C DST DMA Interrupt Disable */
    volatile qe_u32 imr;             /* 0x20 DST DMA Interrupt Mask */
    volatile qe_u32 ctrl2;           /* 0x24 General DST DMA Control Reg 2 */
    volatile qe_u32 dstmsb;          /* 0x28 DMA destination memory address (MSBs) */
} psu_qspi_dma_regs;

typedef struct {
    qe_u8 *txd;
    qe_u8 *rxd;
    qe_u32 length;
    qe_u32 tx_bytes;
    qe_u32 rx_bytes;
} gqspi_transfer;

typedef struct {
    psu_qspi_regs *regs;
    psu_qspi_dma_regs *dma_regs;
    int irq;
    int clock;
    int speed_hz;
    int read_mode;  /* DMA or I/O */
} psu_qspi_dev;

static psu_qspi_dev spi0_dev = {
    .regs = (psu_qspi_regs *)(ZYNQ_QSPI_BASE + GQSPI_REG_OFFSET),
    .dma_regs = (psu_qspi_dma_regs *)(ZYNQ_QSPI_BASE + GQSPI_DMA_REG_OFFSET),
    .clock = 299997000,
    .read_mode = GQSPI_READMODE_IO,
};

static qe_spi_bus spi0_bus;
static qe_spi_dev spi0_device0;



static void set_gen_fifo(psu_qspi_regs *regs, qe_u32 val)
{
    int timeout = 10;

    while (!(regs->isr & GQSPI_ISR_GFIFO_EMPTY))
    ;

    // while (timeout--) {
    //     if (regs->isr & GQSPI_ISR_GFIFO_EMPTY) {
    //         break;
    //     }
    //     qe_usleep(1000);
    // }

    if (!timeout) {
        qspi_warning("write genfifo timeout");
    }

    regs->genfifo = val;
    //qspi_debug("genfifo:0x%x", val);
}

static void psu_qspi_enable(psu_qspi_regs *regs, qe_bool on)
{
    if (on) {
        regs->enr = GQSPI_EN_ENABLE;
        qspi_debug("enable");
    } else {
        regs->enr = 0x0;
        qspi_debug("disable");
    }
}

static void psu_qpsi_abort(psu_qspi_dev *qspi)
{
    qe_u32 isr;
    psu_qspi_regs *regs = qspi->regs;
    psu_qspi_dma_regs *dma_regs = qspi->dma_regs;

    isr = regs->isr;

    /* Clear and disable interrupts */
    regs->isr = isr | GQSPI_ISR_WTC_MASK;
    dma_regs->isr = dma_regs->isr;
    dma_regs->sts |= GQSPI_DMA_STS_WTC;
    regs->idr = GQSPI_IDR_ALL_MASK;
    dma_regs->idr = GQSPI_DMA_IDR_ALL_MASK;

    /* Clear FIFO */
    if (regs->isr & GQSPI_ISR_RX_EMPTY) {
        regs->gqfifoctrl = GQSPI_FIFO_CTRL_RST_TX | 
            GQSPI_FIFO_CTRL_RST_GEN;
    }

	/*
	 * Switch to IO mode to Clear RX FIFO. This is becuase of DMA behaviour
	 * where it waits on RX empty and goes busy assuming there is data
	 * to be transfered even if there is no request.
	 */
    if (isr & GQSPI_ISR_RX_EMPTY) {
        regs->cr &= ~GQSPI_CR_MODE_EN_MASK;
        regs->gqfifoctrl = GQSPI_FIFO_CTRL_RST_RX;
        if (qspi->read_mode == GQSPI_READMODE_DMA) {
            regs->cr |= GQSPI_CR_MODE_EN_DMA;
        }
    }

    /* Disable QSPIPSU */
    psu_qspi_enable(regs, qe_false);
}

static void psu_qspi_set_clk_prescaler(psu_qspi_dev *qspi, 
    qe_u8 prescaler)
{
    qe_u32 cr;
    psu_qspi_regs *regs = qspi->regs;

    cr = regs->cr;

    cr &= ~GQSPI_CR_BAUD_DIV_MASK;
    cr |= (prescaler & GQSPI_CR_PRESC_MAX) << GQSPI_CR_BAUD_DIV_SHIFT;

    regs->cr = cr;
    qspi_debug("set clk prescaler:%d cr:0x%x", prescaler, cr);
}

static void psu_qspi_reset(psu_qspi_dev *qspi)
{
    qe_u32 cr;
    psu_qspi_regs *regs = qspi->regs;
    psu_qspi_dma_regs *dma_regs = qspi->dma_regs;

    /* Abort any transfer that is in progress */
    psu_qpsi_abort(qspi);

    /* Default value to config register */
    cr = regs->cr;

    /* I/O mode */
    cr &= ~GQSPI_CR_MODE_EN_MASK;
    /* Manual start */
    cr |= GQSPI_CR_START_MODE_MANU;
    /* Little endain by default */
    cr &= ~GQSPI_CR_ENDIAN;
    /* Disable poll timeout */
    cr &= ~GQSPI_CR_EN_POOL_TO;
    /* Set hold bit */
    cr |= GQSPI_CR_WP_HOLD;
    /* Clear prescalar by default */
    cr &= ~GQSPI_CR_BAUD_DIV_MASK;
    /* CPOL CPHA 00 */
    cr &= ~GQSPI_CR_CLK_PHA;
    cr &= ~GQSPI_CR_CLK_POL;

    regs->cr = cr;
    qspi_debug("reset set cr:0x%x", cr);
    /* Set by default to allow for high frequencies */
    regs->lpbkdly |= GQSPI_LPBK_DLY_ADJ_LPBK;

    /* Reset thresholds */
    regs->txftr = GQSPI_TX_FIFO_THR_RST_VAL;
    regs->rxftr = GQSPI_RX_FIFO_THR_RST_VAL;
    regs->gfthr = GQSPI_GEN_FIFO_THR_RST_VAL;

    /* DMA init */
    dma_regs->ctrl = GQSPI_DMA_DST_CTRL_RST_VAL;
}

static qe_unused qe_ret psu_qspi_set_speed(psu_qspi_dev *qspi, int speed)
{
    qe_u32 cr;
    qe_u8 baud = 0;
    psu_qspi_regs *regs = qspi->regs;

    if (speed > qspi->clock)
        speed = qspi->clock;

    /* Set the clock frequency */
    cr = regs->cr;
    if (speed == 0) {
        /* Set baudrate x8, if the freq is 0 */
        baud = GQSPI_DFLT_BAUD_RATE_VAL;
    } else if (qspi->speed_hz != speed) {
        while ((baud <= GQSPI_CR_PRESC_MAX) &&
               (qspi->clock / (2 << baud)) > speed)
            baud++;
        if (baud > GQSPI_MAX_BAUD_RATE_VAL)
            baud = GQSPI_DFLT_BAUD_RATE_VAL;
    }

    cr &= ~GQSPI_CR_BAUD_DIV_MASK;
    cr |= (baud << GQSPI_CR_BAUD_DIV_SHIFT);

    regs->cr = cr;

    qspi_debug("speed:%d cr:0x%x baud:%d", speed, cr, baud);

    return qe_ok;
}

static qe_unused qe_ret psu_qspi_set_mode(psu_qspi_dev *qspi, int mode)
{
    qe_u32 cr;
    psu_qspi_regs *regs = qspi->regs;

    /* Set the SPI Clock phase and polarities */
    cr = regs->cr;
    cr &= ~(GQSPI_CR_CLK_PHA | GQSPI_CR_CLK_POL);
        
    if (mode & QE_SPI_CPHA)
        cr |= GQSPI_CR_CLK_PHA;
    if (mode & QE_SPI_CPOL)
        cr |= GQSPI_CR_CLK_POL;

    regs->cr = cr;

    qspi_debug("mode:%d cr:0x%x", mode, cr);

    return qe_ok;
}

static void cs_assert(psu_qspi_dev *qspi)
{
    qe_u32 r = 0x0;
    psu_qspi_regs *regs = qspi->regs;

    r &= ~(GQSPI_GENFIFO_XFER | GQSPI_GENFIFO_EXP);
    r &= ~GQSPI_GENFIFO_MODE_MASK;
    r |= GQSPI_GENFIFO_MODE_SPI;
    r |= GQSPI_GENFIFO_CS_LOWER;
    r &= ~GQSPI_GENFIFO_BUS_MASK;
    r |= GQSPI_GENFIFO_BUS_LOWER;
    r &= ~(GQSPI_GENFIFO_TX | GQSPI_GENFIFO_RX |
        GQSPI_GENFIFO_STRIPE | GQSPI_GENFIFO_POLL);
    r |= GQSPI_GENFIFO_CS_SETUP;

    qspi_debug("cs assert");
    set_gen_fifo(regs, r);
}

static void cs_deassert(psu_qspi_dev *qspi)
{
    qe_u32 r = 0x0;
    psu_qspi_regs *regs = qspi->regs;

	r = 0x0U;
	r &= ~(GQSPI_GENFIFO_XFER | GQSPI_GENFIFO_EXP);
	r &= ~GQSPI_GENFIFO_MODE_MASK;
	r |= GQSPI_GENFIFO_MODE_SPI;
	r &= ~GQSPI_GENFIFO_BUS_MASK;
	r |= GQSPI_GENFIFO_BUS_LOWER;
	r &= ~(GQSPI_GENFIFO_TX | GQSPI_GENFIFO_RX |
			GQSPI_GENFIFO_STRIPE | GQSPI_GENFIFO_POLL);
	r |= GQSPI_GENFIFO_CS_HOLD;

    //qspi_debug("cs deassert");
    set_gen_fifo(regs, r);
}

static qe_ret psu_qspi_configure(qe_spi_dev *spi,
	qe_spi_configure *cfg)
{
	return qe_ok;
}

static int calc_exponent(gqspi_transfer *transfer, qe_u32 *val)
{
    int len;
    qe_u32 exp_val = 8;

    while (1) {
        if (transfer->length > 255) {
            if (transfer->length & (1 << exp_val)) {
				*val &= ~GQSPI_GENFIFO_IMM_DATA_MASK;
				*val |= GQSPI_GENFIFO_EXP;
				*val |= exp_val;
				transfer->length -= (1 << exp_val);
				return exp_val;
            }
            exp_val++;
        } else {
			*val &= ~(GQSPI_GENFIFO_IMM_DATA_MASK |
					  GQSPI_GENFIFO_EXP);
			*val |= (qe_u8)transfer->length;
			len = (int)transfer->length;
			transfer->length  = 0;
			return len;
        }
    }
}

static void fill_tx_fifo(psu_qspi_regs *regs, gqspi_transfer *transfer, int size)
{
    qe_u32 data;
    qe_u32 *words = (qe_u32 *)transfer->txd;

    while (size) {

        while (regs->isr & GQSPI_ISR_TX_FULL)
            ;

        if (size >= 4) {
            regs->txd0r = *words;
            words++;
            size -= 4;
        } else {
            switch (size) {
			case 1:
				data = *((qe_u8 *)words);
				words += 1;
				data |= QE_GENMASK(31, 8);
				break;
			case 2:
				data = *((qe_u16 *)words);
				words += 2;
				data |= QE_GENMASK(31, 16);
				break;
			case 3:
				data = *((qe_u16 *)words);
				words += 2;
				data |= (*((qe_u8 *)words) << 16);
				words += 1;
				data |= QE_GENMASK(31, 24);
				break;
			}
            regs->txd0r = data;
			size = 0;
        }
    }
}

static qe_unused void genfifo_tx_transfer(psu_qspi_dev *qspi,
	gqspi_transfer *transfer)
{
    int len;
    qe_u32 val;
    psu_qspi_regs *regs = qspi->regs;

    val = GQSPI_GENFIFO_BUS_LOWER | GQSPI_GENFIFO_CS_LOWER;
    val |= GQSPI_GENFIFO_TX | GQSPI_GENFIFO_XFER;
    val |= GQSPI_GENFIFO_MODE_SPI;

    while (transfer->length) {
        len = calc_exponent(transfer, &val);
        set_gen_fifo(regs, val);
        if (val & GQSPI_GENFIFO_EXP)
            fill_tx_fifo(regs, transfer, 1 << len);
        else
            fill_tx_fifo(regs, transfer, len);
    }
}

static int dma_align_size(int size, int align)
{
    return ((size + (align - 1)) / align) * align;
}

static qe_ret qspi_dma_transfer_start(psu_qspi_dev *qspi, 
    gqspi_transfer *transfer, qe_u32 genfifo, qe_u32 *buf)
{
    int len;
    int transfer_len = transfer->length;
    qe_u32 size;
    qe_u32 addr;
    psu_qspi_regs *regs = qspi->regs;
    psu_qspi_dma_regs *dma_regs = qspi->dma_regs;

    dma_regs->dst = (qe_u32)buf;
    dma_regs->size = dma_align_size(transfer->length, 128);
    dma_regs->ier = GQSPI_DMA_IER_ALL_MASK;
    addr = (qe_u32)buf;
    size = dma_align_size(transfer->length, 128);
    Xil_DCacheFlushRange(addr, size);

    while (transfer->length) {
        len = calc_exponent(transfer, &genfifo);
        if (!(genfifo & GQSPI_GENFIFO_EXP) &&
            (len % 128)) {
            genfifo &= ~QE_GENMASK(7, 0);
            genfifo |= dma_align_size(len, 128);
        }
        set_gen_fifo(regs, genfifo);
    }

    while (!(dma_regs->isr & GQSPI_DMA_STS_DONE))
        ;
    dma_regs->isr = GQSPI_DMA_STS_DONE;

    qehex_debug("dmarx", buf, dma_align_size(len, 128));

    if ((qe_u8 *)buf != transfer->rxd) {
        qe_memcpy(transfer->rxd, buf, transfer_len);
    }

    return qe_ok;
}

static qe_unused qe_ret genfifo_rx_transfer(psu_qspi_dev *qspi,
	gqspi_transfer *transfer)
{
    //int size;
    qe_u32 val;
    qe_u32 buf;
    qe_u8 tmp[512];

    val = GQSPI_GENFIFO_BUS_LOWER | GQSPI_GENFIFO_CS_LOWER;
    val |= GQSPI_GENFIFO_RX | GQSPI_GENFIFO_XFER;
    val |= GQSPI_GENFIFO_MODE_SPI;

	/*
	 * Check if receive buffer is aligned to 4 byte and length
	 * is multiples of four byte as we are using dma to receive.
	 */
    if (!((qe_base)transfer->rxd & (GQSPI_DMA_ALIGN - 1)) &&
        !(transfer->length % GQSPI_DMA_ALIGN)) {
        buf = *(qe_u32 *)transfer->rxd;
        return qspi_dma_transfer_start(qspi, transfer, val, (qe_u32 *)&buf);
    }

    buf = *(qe_u32 *)tmp;
    qspi_dma_transfer_start(qspi, transfer, val, &buf);
    return qe_ok;
}

static qe_unused void genfifo_cmd_transfer(psu_qspi_dev *qspi,
    gqspi_transfer *transfer)
{
    qe_u32 val;

    while (transfer->length) {
        val = GQSPI_GENFIFO_BUS_LOWER | GQSPI_GENFIFO_CS_LOWER;
        val |= GQSPI_GENFIFO_TX | GQSPI_GENFIFO_MODE_SPI;
        val |= *transfer->txd++;
        transfer->length--;
        set_gen_fifo(qspi->regs, val);
    }
}

static void gqspi_read_rx_fifo(psu_qspi_dev *qspi, 
    gqspi_transfer *transfer, qe_u32 size)
{
    qe_u32 count = 0;
	qe_u32 data = 0;
    psu_qspi_regs *regs = qspi->regs;

    while ((transfer->rx_bytes != 0) && (count < size)) {
        data = regs->drxr;
        if (transfer->rx_bytes >= 4) {
            qe_memcpy(transfer->rxd, &data, 4);
            transfer->rx_bytes -= 4;
            transfer->rxd += 4;
            count += 4;
        } else {
			/* Read unaligned bytes (< 4 bytes) */
			qe_memcpy(transfer->rxd, &data, transfer->rx_bytes);
			transfer->rxd += transfer->rx_bytes;
			count += transfer->rx_bytes;
			transfer->rx_bytes = 0;
        }
    }
}

static void gqspi_fill_tx_fifo(psu_qspi_dev *qspi, 
    gqspi_transfer *transfer, qe_u32 size)
{
    int count = 0;
    qe_u32 data = 0;
    psu_qspi_regs *regs = qspi->regs;

    while ((transfer->tx_bytes > 0) && (count < size)) {
        if (transfer->tx_bytes >= 4) {
            qe_memcpy(&data, transfer->txd, 4);
            transfer->txd += 4;
            transfer->tx_bytes -= 4;
            count += 4;
        } else {
            qe_memcpy(&data, transfer->txd, transfer->tx_bytes);
            transfer->txd += transfer->tx_bytes;
            count += transfer->tx_bytes;
            transfer->tx_bytes = 0;
        }
        qspi_debug("tx 0x%x", data);
        regs->txd0r = data;
    }
}

static void gqspi_setup_dma(psu_qspi_dev *qspi, 
    gqspi_transfer *transfer)
{
    qe_u32 remain;
    qe_u32 dma_rx_bytes;
    qe_u64 addr_tmp;
    psu_qspi_dma_regs *dma_regs = qspi->dma_regs;

    addr_tmp = (qe_u64)((qe_base)transfer->rxd & GQSPI_DMA_DST_ADDR_MASK);
    /* Check for RXBfrPtr to be word aligned */
    dma_regs->dst = (qe_u32)addr_tmp;

    
#ifdef __aarch64__
	addr_tmp = (qe_u64)((qe_base)(transfer->rxd) >> 32);
    dma_regs->dstmsb = addr_tmp & GQSPI_DMA_DST_ADDR_MSB_MASK;
#endif

    remain = transfer->rx_bytes % 4;
    dma_rx_bytes = transfer->rx_bytes;
    if (remain != 0) {
        /* This is done to make Dma bytes aligned */
        dma_rx_bytes = transfer->rx_bytes - remain;
        transfer->length = dma_rx_bytes;
    }

    /* Write no. of words to DMA DST SIZE */
    dma_regs->size = dma_rx_bytes;
}

static void gqspi_txrx_setup(psu_qspi_dev *qspi, 
    gqspi_transfer *transfer, qe_u32 *genfifo)
{

    if (transfer->txd && !transfer->rxd) {
        /* Setup data to be TXed */
        *genfifo |= GQSPI_GENFIFO_XFER | GQSPI_GENFIFO_TX;
        transfer->tx_bytes = transfer->length;
        gqspi_fill_tx_fifo(qspi, transfer, GQSPI_FIFO_DEPTH);
        /* Discard RX data */
        *genfifo &= ~GQSPI_GENFIFO_RX;
        transfer->rx_bytes = 0;
    }

    if (transfer->rxd && !transfer->txd) {
        /* TX auto fill */
        *genfifo &= ~GQSPI_GENFIFO_TX;
        transfer->tx_bytes = 0;
        /* Setup RX */
		*genfifo |= GQSPI_GENFIFO_XFER;
		*genfifo |= GQSPI_GENFIFO_RX;
        transfer->rx_bytes = transfer->length;
        if (qspi->read_mode == GQSPI_READMODE_DMA) {
            gqspi_setup_dma(qspi, transfer);
        }
    }
}

static void genfifo_push_transfer(psu_qspi_dev *qspi, 
    gqspi_transfer *transfer)
{
    int count;
    qe_u32 gfifo = 0x0;
    qe_u32 immd;
    psu_qspi_regs *regs = qspi->regs;

    gfifo |= GQSPI_GENFIFO_BUS_LOWER | GQSPI_GENFIFO_CS_LOWER | GQSPI_GENFIFO_MODE_SPI;
    
    /* If Byte Count is less than 8 bytes do the transfer in IO mode */
    if ((transfer->length < 8) && (qspi->read_mode == GQSPI_READMODE_DMA)) {
        qspi->read_mode = GQSPI_READMODE_IO;
        regs->cr &= ~GQSPI_CR_MODE_EN_MASK;
    }

    gqspi_txrx_setup(qspi, transfer, &gfifo);

    if (transfer->length < 0xFF) {
        gfifo &= ~GQSPI_GENFIFO_IMM_DATA_MASK;
        gfifo |= transfer->length;
        qspi_debug("genfifo 0x%x", gfifo);
        regs->genfifo = gfifo;
    } else {
        count = transfer->length;
        qe_u32 exponent = 8;
        immd = count & 0xFF;
        /* Exponent entries */
        gfifo |= GQSPI_GENFIFO_EXP;
        while (count) {
            if (count & GQSPI_GENFIFO_EXP_START) {
                gfifo &= ~GQSPI_GENFIFO_IMM_DATA_MASK;
                gfifo |= exponent;
                qspi_debug("genfifo 0x%x", gfifo);
                regs->genfifo = gfifo;
            }
            count = count >> 1;
            exponent++;
        }

        /* Immediate entry */
        gfifo &= ~GQSPI_GENFIFO_EXP;
        if (immd & 0xFFU) {
            gfifo &= ~GQSPI_GENFIFO_IMM_DATA_MASK;
            gfifo |= immd & 0xFF;
            qspi_debug("genfifo 0x%x", gfifo);
            regs->genfifo = gfifo;
        }
    }

    /* One dummy GenFifo entry in case of IO mode */
    if ((qspi->read_mode == GQSPI_READMODE_IO) && 
        (transfer->rxd)) {
        gfifo = 0x0;
        qspi_debug("dummy genfifo 0x%x", gfifo);
        regs->genfifo = gfifo;
    }
}

static qe_ret psu_qspi_xfer(qe_spi_dev *dev,
	qe_spi_message *message)
{
    qe_u32 rxthr;
    gqspi_transfer transfer;
    psu_qspi_dev *qspi = (psu_qspi_dev *)dev->parent.priv;
    psu_qspi_regs *regs = qspi->regs;

    transfer.txd    = (qe_u8 *)message->send_buf;
    transfer.rxd    = (qe_u8 *)message->recv_buf;
    transfer.length = message->length;
    transfer.rx_bytes = 0;
    transfer.tx_bytes = 0;

    if (message->length > GQSPI_DMA_BYTES_MAX) {
        qspi_error("transfer %d > %d", message->length, GQSPI_DMA_BYTES_MAX);
        return -1;
    }

    qspi_debug("transfer %p %p %d",
        transfer.txd, 
        transfer.rxd,
        transfer.length);

    if (message->send_buf) {
    	qspi_debug("send buf:");
    	qehex_debug(QSPI_LOGDOMAIN, message->send_buf, message->length);
    }

    /* cs take */
    if (message->cs_take) {
        psu_qspi_enable(regs, qe_true);
        cs_assert(qspi);
    }

    genfifo_push_transfer(qspi, &transfer);
    regs->cr |= GQSPI_CR_START_GENFIFO;
    
    /* Use thresholds here */
    /* If there is more data to be transmitted */
    do {
        if ((regs->isr & GQSPI_ISR_TX_NOT_FULL) && 
            (transfer.txd) && 
            (transfer.tx_bytes>0)) {
            gqspi_fill_tx_fifo(qspi, &transfer, GQSPI_FIFO_DEPTH);
        }

        /* Check if DMA RX is complete and update RxBytes */
        if (transfer.rxd) {
            rxthr = regs->rxftr;
            qspi_debug("rxthr %d isr 0x%x", rxthr, regs->isr);
            if (regs->isr & GQSPI_ISR_RX_NOT_EMPTY) {
                gqspi_read_rx_fifo(qspi, &transfer, rxthr*4);
            } else if (regs->isr & GQSPI_ISR_GFIFO_EMPTY) {
                gqspi_read_rx_fifo(qspi, &transfer, transfer.rx_bytes);
            }
        }
    } while (!(regs->isr & GQSPI_ISR_GFIFO_EMPTY) ||
             (transfer.tx_bytes != 0) ||
             !(regs->isr & GQSPI_ISR_TX_EMPTY) ||
             (transfer.rx_bytes != 0));

    /* cs release */
    if (message->cs_release) {
        cs_deassert(qspi);
        regs->cr |= GQSPI_CR_START_GENFIFO;
        while (!(regs->isr & GQSPI_ISR_GFIFO_EMPTY))
            ;
        psu_qspi_enable(regs, qe_false);
    }

    if (message->recv_buf) {
        qspi_debug("recv buf:");
        qehex_debug(QSPI_LOGDOMAIN, message->recv_buf, message->length);
    }

    return message->length;
}

static qe_spi_ops psu_qspi_ops = {
    .configure = psu_qspi_configure,
    .xfer = psu_qspi_xfer,
};

static void psu_qspi_dev_init(psu_qspi_dev *qspi)
{
    psu_qspi_regs *regs = qspi->regs;

    /* Controller select generic */
    regs->gqspisel = GQSPI_SEL_GQSPI;

    /*
     * Reset the QSPIPSU device to get it into its initial state. It is
     * expected that device configuration will take place after this
     * initialization is done, but before the device is started.
     */
    psu_qspi_reset(qspi);

    /* Set the prescaler for QSPIPSU clock */
    psu_qspi_set_clk_prescaler(qspi, GQSPI_CLK_PRESCALE_8);
}

static qe_init int psu_qspi_init(void)
{
    qelog_domain_set_level(QSPI_LOGDOMAIN, QELOG_WARNING);

    /* do hardware spi init */
    psu_qspi_dev_init(&spi0_dev);

    /* register spi bus */
    qe_spi_bus_register(&spi0_bus, "spi0", &psu_qspi_ops);

    /* attach spi device to spi bus */
    qe_spi_bus_attach_device(&spi0_device0, "spi0.0", "spi0", &spi0_dev);
    return 0;
}
QE_BOARD_EXPORT(psu_qspi_init);
#endif
